/* i2c - Simple example

   Simple I2C example that shows how to initialize I2C
   as well as reading and writing from and to registers for a sensor connected over I2C.

   The sensor used in this example is a MPU9250 inertial measurement unit.

   For other examples please check:
   https://github.com/espressif/esp-idf/tree/master/examples

   See README.md file to get detailed usage of this example.

   This example code is in the Public Domain (or CC0 licensed, at your option.)

   Unless required by applicable law or agreed to in writing, this
   software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
   CONDITIONS OF ANY KIND, either express or implied.
*/
#include <stdio.h>
#include "esp_log.h"
#include "driver/i2c.h"
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/event_groups.h"

#include "mpu6050.h"

static const char *TAG = "i2c-simple-example";

#define I2C_MASTER_SCL_IO           15      /*!< GPIO number used for I2C master clock */
#define I2C_MASTER_SDA_IO           18      /*!< GPIO number used for I2C master data  */
#define I2C_MASTER_NUM              0                          /*!< I2C master i2c port number, the number of i2c peripheral interfaces available will depend on the chip */
#define I2C_MASTER_FREQ_HZ          400000                     /*!< I2C master clock frequency */
#define I2C_MASTER_TX_BUF_DISABLE   0                          /*!< I2C master doesn't need buffer */
#define I2C_MASTER_RX_BUF_DISABLE   0                          /*!< I2C master doesn't need buffer */
#define I2C_MASTER_TIMEOUT_MS       1000

#define MPU9250_SENSOR_ADDR                 0x68        /*!< Slave address of the MPU9250 sensor */
#define MPU9250_WHO_AM_I_REG_ADDR           0x75        /*!< Register addresses of the "who am I" register */

#define MPU9250_PWR_MGMT_1_REG_ADDR         0x6B        /*!< Register addresses of the power managment register */
#define MPU9250_RESET_BIT                   7
#define MOVE_2

/**
 * @brief i2c master initialization
 */
static esp_err_t i2c_master_init(void)
{
    int i2c_master_port = I2C_MASTER_NUM;

    i2c_config_t conf = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = I2C_MASTER_SDA_IO,
        .scl_io_num = I2C_MASTER_SCL_IO,
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master.clk_speed = I2C_MASTER_FREQ_HZ,
    };

    i2c_param_config(i2c_master_port, &conf);

    return i2c_driver_install(i2c_master_port, conf.mode, I2C_MASTER_RX_BUF_DISABLE, I2C_MASTER_TX_BUF_DISABLE, 0);
}

uint8_t getCirclePos(double angle)
{
	//角度从-180°到180°，一圈360°分成36格，每转过10°算一格	
	return (uint8_t)((angle - (-180.0)) / 10);
}

void app_main(void)
{
	double maxangle = 0, minangle = 0;	
	uint32_t mintimestamp = 0, maxtimestamp = 0;
	double rolltemp,pitchtemp;
	
	double currentangel;
	uint8_t tracedir = 0;
	uint8_t lasttracedir = 0;
	uint32_t move1cnt = 0;
	
	uint32_t circlestartstamp,circleendstamp;
	uint32_t stepstartstamp,stependstamp;
	uint8_t lastpos = 0, curpos = 0;
	uint8_t circletrace = 0;
	uint8_t circlecnt = 0;
	uint32_t move2cnt = 0;
	uint8_t backward = 0;
	
	MPU6050_t mdata;
    i2c_master_init();
	
	MPU6050_Init();
	MPU6050_Read_All(&mdata,&rolltemp,&pitchtemp);
	currentangel = mdata.KalmanAngleX;
	maxangle = currentangel;
	minangle = currentangel;
	mintimestamp = maxtimestamp = xTaskGetTickCount()*portTICK_PERIOD_MS;
	
	lastpos = curpos = getCirclePos(currentangel);
	circletrace = 1;
	circlecnt = 0;
	circlestartstamp = circleendstamp = xTaskGetTickCount()*portTICK_PERIOD_MS;
	stepstartstamp = stependstamp = xTaskGetTickCount()*portTICK_PERIOD_MS;
	
	while(1)
	{
		vTaskDelay(10/portTICK_PERIOD_MS);
		
		MPU6050_Read_All(&mdata,&rolltemp,&pitchtemp);
		currentangel = pitchtemp;//mdata.KalmanAngleY;
		//ESP_LOGI(TAG, "currentangel:%f", currentangel);
		#ifdef MOVE_1
		//case 1,front back,0.5s~1.0s,20°~60°,minangle->maxangle->minangle
		if(currentangel > maxangle) {//如果当前角度大于记录的最大角度，则更新最大角度和相应时间戳
			maxangle = currentangel;
			maxtimestamp = xTaskGetTickCount()*portTICK_PERIOD_MS;
		}
		if(currentangel < minangle) {//如果当前角度小于记录的最小角度，则更新最小角度和相应时间戳
			minangle = currentangel;
			mintimestamp = xTaskGetTickCount()/portTICK_PERIOD_MS;
		}
		if(maxangle - minangle > 20 && maxangle - minangle < 60) {//如果最大角度和最小角度相差在20°到60°之间，则可能是有效运动
			if(abs(maxtimestamp - mintimestamp) > 100 && abs(maxtimestamp - mintimestamp) < 1000) {//判断最大最小时间戳，防止运动太快或太慢
				if(maxtimestamp > mintimestamp) {//判断运动趋势，即向前运动或向后运动
					tracedir = 1;
				} else {
					tracedir = 2;
				}
				//上一次向前运动，则这次向后运动为有效运动；上一次向后运动，则这一次向前运动为有效运动
				if((lasttracedir == 1 && tracedir == 2) || (lasttracedir == 2 && tracedir == 1)) {
					move1cnt++;
					if(move1cnt & 0x01 == 0) {//向前向后都算一次，所以总次数要除以二
						ESP_LOGI(TAG, "move 1 cnt:%d", move1cnt >> 1);
					}					
				}				
			}
			else {
				ESP_LOGI(TAG, "time gap fail:%d", abs(maxtimestamp - mintimestamp));
			}
			lasttracedir = tracedir;//保存本次运动趋势，并更新最大最小角度和时间戳，进行新一轮判断
			if(maxtimestamp > mintimestamp) {
				mintimestamp = maxtimestamp;
				minangle = maxangle;
			} else {
				maxtimestamp = mintimestamp;
				maxangle = minangle;
			}
		} else {			
		}
		#endif
		#ifdef MOVE_2
		curpos = getCirclePos(currentangel);//获取当前角度所占的格子位置
		stependstamp = xTaskGetTickCount()*portTICK_PERIOD_MS;//更新单元格时间
		circleendstamp = xTaskGetTickCount()*portTICK_PERIOD_MS;//更新绕圈时间
		//ESP_LOGI(TAG, "curpos:%d, %f", curpos, currentangel);
		if(curpos != lastpos) {
			ESP_LOGI(TAG, "curpos:%d", circlecnt);
		}
		if(curpos == lastpos) {//格子位置不变，无需处理
			
		} else if(((lastpos + 1) % 36) == curpos) {//格子正向走了一格
			if(circletrace == 1) {//上一次格子如果也是正向运动的
				if(stependstamp - stepstartstamp < 1000 && stependstamp - stepstartstamp > 5) {//判断这一格走了多长时间，太长的则忽略，否则这一格有效					
					circlecnt++;//总格数累加
					backward = 0;
				} else {//
					circlecnt = 0;
					circlestartstamp = circleendstamp;
				}				
			} else {//如果上一次格子是反向运动的，标记一下，如果连续出现两次反向运动，则清空当前运动的计数，并开始反向计数
				//ESP_LOGI(TAG, "backward");
				backward ++;
				if(circlecnt > 1)
					circlecnt -= 1;
				if(backward >= 2) {					
					circletrace = 1;//更新运动趋势
					backward = 0;
					circlecnt = 0;
					circlestartstamp = circleendstamp;
				}
			}
			lastpos = curpos;//保存当前格子位置
			stepstartstamp = stependstamp;//保存当前格子结束时间			
		} else if(((curpos + 1) % 36) == lastpos) {//格子反向走了一格
			if(circletrace == 2) {//上一次格子如果也是反向运动的
				if(stependstamp - stepstartstamp < 1000 && stependstamp - stepstartstamp > 5) {//判断这一格走了多长时间，太长的则忽略，否则这一格有效					
					circlecnt++;//总格数累加
					backward = 0;
				} else {
					circlecnt = 0;
					circlestartstamp = circleendstamp;
				}
				stepstartstamp = stependstamp;
			} else {//如果上一次格子是反向运动的，标记一下，如果连续出现两次反向运动，则清空当前运动的计数，并开始反向计数
				//ESP_LOGI(TAG, "backward");
				backward ++;
				if(circlecnt > 1)
					circlecnt -= 1;
				if(backward >= 2) {
					circletrace = 2;//更新运动趋势
					backward = 0;
					circlecnt = 0;
					circlestartstamp = circleendstamp;
				}
			}
			lastpos = curpos;//保存当前格子位置
			stepstartstamp = stependstamp;			
		} else {
			circlecnt = 0;
			lastpos = curpos;
			stepstartstamp = stependstamp;
			circlestartstamp = circleendstamp;
		}
		
		if(circlecnt >= 30) {//如果记步总数达到30以上，可以认为已经绕了一圈了
			if((circleendstamp - circlestartstamp > 100) && (circleendstamp - circlestartstamp < 100000) ) {//计算一下这圈走了多少时间
				move2cnt++;
				ESP_LOGI(TAG, "====================================move 2 cnt:%d", move2cnt);
			}
			//重新开始新的一圈的计数
			circlecnt = 0;
			lastpos = curpos;
			stepstartstamp = stependstamp;
			circlestartstamp = circleendstamp;
		}
		#endif
	}
}
